import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Pose, Twist
import sys

class Get_local_position(Node):
    def __init__(self,vehicle_type,vehicle_id):
        self.timer_period=0.3
        self.vehicle_id=vehicle_id
        self.vehicle_type=vehicle_type
        super().__init__(self.vehicle_type+'_'+self.vehicle_id+"get_local_position") 
        self.local_pose_sub = self.create_subscription( PoseStamped, self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose",self.local_position_callback, 1)
        self.local_pose_pub=self.create_publisher(PoseStamped, self.vehicle_type+'_'+self.vehicle_id+"/xtdrone/local_position/pose",1)

    def local_position_callback(self,msg):
        self.local_pose_pub.publish(msg)

if __name__ == '__main__':
    rclpy.init()
    get_position=Get_local_position(sys.argv[1],sys.argv[2])    
    rclpy.spin(get_position)
    rclpy.shutdown()



